/*
 * Copyright (c) 2006-2022, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2018-10-23     flybreak     the first version
 */

#include <rtthread.h>
#include "mpu6xxx.h"
#include <stdio.h>

/* Default configuration, please change according to the actual situation, support i2c and spi device name */
#define MPU6050_DEVICE_NAME  "i2c1"

/* Test function */
static int mpu6050_test(int argc, char**argv)
{
    struct mpu6xxx_device *dev;
    struct mpu6xxx_3axes accel, gyro;
    int i;
    int n = 5;
    int d = 100;

    /* Initialize mpu6xxx, The parameter is RT_NULL, means auto probing for i2c*/
    dev = mpu6xxx_init(MPU6050_DEVICE_NAME, RT_NULL);

    if (dev == RT_NULL)
    {
        rt_kprintf("mpu6050 init failed\n");
        return -1;
    }
    rt_kprintf("mpu6050 init succeed\n");

    if(argc>=2) {
      sscanf(argv[1], "%d", &n);
      if(n<1) {
          n = 1;
      }
    }
    if(argc>=3) {
      sscanf(argv[2], "%d", &d);
      if(d<100) {
          d = 100;
      }
    }
    rt_kprintf("read %d times, delay %d ms\n", n, d);

    for (i = 0; i < n; i++)
    {
        mpu6xxx_get_accel(dev, &accel);
        mpu6xxx_get_gyro(dev, &gyro);

        rt_kprintf("accel: x = %3d, y = %3d, z = %3d\t", accel.x, accel.y, accel.z);
        rt_kprintf("gyro:  x = %3d, y = %3d, z = %3d\n", gyro.x, gyro.y, gyro.z);

        rt_thread_mdelay(d);
    }

    mpu6xxx_deinit(dev);

    return 0;
}
MSH_CMD_EXPORT(mpu6050_test, mpu6050 sensor test function);
